#include "swegl/math.h"

namespace swegl
{

const Matrix3x3 Matrix3x3::Zero(Vec3(0, 0, 0), Vec3(0, 0, 0), Vec3(0, 0, 0));
const Matrix3x3 Matrix3x3::Identity(Vec3(1, 0, 0), Vec3(0, 1, 0), Vec3(0, 0, 1));

Matrix3x3 Matrix3x3::operator * (const Matrix3x3 &rhs) const
{
	return Matrix3x3(
	           m[0].x * rhs[0].x + m[0].y * rhs[1].x + m[0].z * rhs[2].x,
	           m[0].x * rhs[0].y + m[0].y * rhs[1].y + m[0].z * rhs[2].y,
	           m[0].x * rhs[0].z + m[0].y * rhs[1].z + m[0].z * rhs[2].z,

	           m[1].x * rhs[0].x + m[1].y * rhs[1].x + m[1].z * rhs[2].x,
	           m[1].x * rhs[0].y + m[1].y * rhs[1].y + m[1].z * rhs[2].y,
	           m[1].x * rhs[0].z + m[1].y * rhs[1].z + m[1].z * rhs[2].z,

	           m[2].x * rhs[0].x + m[2].y * rhs[1].x + m[2].z * rhs[2].x,
	           m[2].x * rhs[0].y + m[2].y * rhs[1].y + m[2].z * rhs[2].y,
	           m[2].x * rhs[0].z + m[2].y * rhs[1].z + m[2].z * rhs[2].z
			   );
}

Matrix3x3 Matrix3x3::operator + (const Matrix3x3 &rhs) const
{
	return Matrix3x3(m[0] + rhs[0], m[1] + rhs[1], m[2] + rhs[2]);
}

Matrix3x3 Matrix3x3::operator - (const Matrix3x3 &rhs) const
{
	return Matrix3x3(m[0] - rhs[0], m[1] - rhs[1], m[2] - rhs[2]);
}

Matrix3x3 &Matrix3x3::operator *= (const Matrix3x3 &rhs)
{
	*this = *this * rhs;
	return *this;
}

Matrix3x3 &Matrix3x3::operator += (const Matrix3x3 &rhs)
{
	m[0] += rhs[0];
	m[1] += rhs[1];
	m[2] += rhs[2];
	return *this;
}

Matrix3x3 &Matrix3x3::operator -= (const Matrix3x3 &rhs)
{
	m[0] -= rhs[0];
	m[1] -= rhs[1];
	m[2] -= rhs[2];
	return *this;
}

Matrix3x3 &Matrix3x3::transpose()
{
	Swap(m[0].y, m[1].x);
	Swap(m[0].z, m[2].x);
	Swap(m[1].z, m[2].y);
	return *this;
}

Matrix3x3 Matrix3x3::Transpose(const Matrix3x3 &m)
{
	return Matrix3x3(m[0].x, m[1].x, m[2].x,
	                 m[0].y, m[1].y, m[2].y,
	                 m[0].z, m[1].z, m[2].z);
}

bool Matrix3x3::inverse()
{
	Matrix3x3 inverse;
	float det, invDet;

	inverse[0][0] = m[1][1] * m[2][2] - m[1][2] * m[2][1];
	inverse[1][0] = m[1][2] * m[2][0] - m[1][0] * m[2][2];
	inverse[2][0] = m[1][0] * m[2][1] - m[1][1] * m[2][0];

	det = m[0][0] * inverse[0][0] + m[0][1] * inverse[1][0] + m[0][2] * inverse[2][0];
#define MATRIX_INVERSE_EPSILON 1e-14
	if (Abs(det) < MATRIX_INVERSE_EPSILON)
	{
		return false;
	}

	invDet = 1.0f / det;

	inverse[0][1] = m[0][2] * m[2][1] - m[0][1] * m[2][2];
	inverse[0][2] = m[0][1] * m[1][2] - m[0][2] * m[1][1];
	inverse[1][1] = m[0][0] * m[2][2] - m[0][2] * m[2][0];
	inverse[1][2] = m[0][2] * m[1][0] - m[0][0] * m[1][2];
	inverse[2][1] = m[0][1] * m[2][0] - m[0][0] * m[2][1];
	inverse[2][2] = m[0][0] * m[1][1] - m[0][1] * m[1][0];

	m[0][0] = inverse[0][0] * invDet;
	m[0][1] = inverse[0][1] * invDet;
	m[0][2] = inverse[0][2] * invDet;

	m[1][0] = inverse[1][0] * invDet;
	m[1][1] = inverse[1][1] * invDet;
	m[1][2] = inverse[1][2] * invDet;

	m[2][0] = inverse[2][0] * invDet;
	m[2][1] = inverse[2][1] * invDet;
	m[2][2] = inverse[2][2] * invDet;

	return true;
}
/*
Matrix3x3 Matrix3x3::Inverse(const Matrix3x3 &m)
{
	shTodo();
	return Matrix3x3();
}
*/

void Matrix3x3::setRotationX(float angle)
{
	float sinVal, cosVal;
	SinCos(angle, sinVal, cosVal);
	*this = Identity;
	m[1].y = cosVal;
	m[1].z = sinVal;
	m[2].y = -sinVal;
	m[2].z = cosVal;
}

void Matrix3x3::setRotationY(float angle)
{
	float sinVal, cosVal;
	SinCos(angle, sinVal, cosVal);
	*this = Identity;
	m[0].x = cosVal;
	m[0].z = -sinVal;
	m[2].x = sinVal;
	m[2].z = cosVal;
}

void Matrix3x3::setRotationZ(float angle)
{
	float sinVal, cosVal;
	SinCos(angle, sinVal, cosVal);
	*this = Identity;
	m[0].x = cosVal;
	m[0].y = sinVal;
	m[1].x = -sinVal;
	m[1].y = cosVal;
}

void Matrix3x3::setRotation(const Vec3 &axis, float angle)
{
	float sinVal, cosVal;
	SinCos(angle, sinVal, cosVal);
	float omc = 1 - cosVal;

	float x = axis.x;
	float y = axis.y;
	float z = axis.z;

	*this = Matrix3x3(
	            x * x * omc + cosVal,     y * x * omc + z * sinVal, x * z * omc - y * sinVal,
	            x * y * omc - z * sinVal, y * y * omc + cosVal,     y * z * omc + x * sinVal,
	            x * z * omc + y * sinVal, y * z * omc - x * sinVal, z * z * omc + cosVal);
}

}

